from machine import I2C,UART,Pin
from  Sentry3  import *
import time

# 等待Sentry3完成操作系统的初始化。此等待时间不可去掉，避免出现Sentry3尚未初始化完毕主控器已经开发发送指令的情况
time.sleep(2)

# 选择UART或者I2C通讯模式，Sentry3出厂默认为I2C模式，短按模式按键可以切换
# 4种UART通讯模式：UART9600（标准协议指令），UART57600（标准协议指令），UART115200（标准协议指令），Simple9600（简单协议指令），
# 参看“简单协议指令”
# https://tosee.readthedocs.io/zh/latest/Common/Protocol/index.html#section-6
#########################################################################################################
# port = UART(2,rx=Pin(16),tx=Pin(17),baudrate=9600)
port = I2C(1,scl=Pin(22),sda=Pin(21),freq=400000)

# Sentry3通讯地址：0x60。如果I2C总线挂接多个设备，请避免出现地址冲突
Sentry3 = Sentry3(0x60)

err = Sentry3.begin(port)
print("Sentry3.begin: 0x%x"% err)

# 设置输出线段的数量，算法默认设置为1
Sentry3.VisionSetMode(sentry3_vision_e.kVisionLine,1)
 
# 1、算法文档
# https://tosee.readthedocs.io/zh/latest/Sentry2/Vision/index.html#chapter-vision-line-index
# 2、请保持默认设置：只检测一条线段
# 3、正常使用时，应由主控器发送指令控制Sentry3算法的开启与关闭，而非通过摇杆手动进行操作；
# 4、满足特定限制条件下，Sentry3可以并行运行多个识别算法
err = Sentry3.VisionBegin(sentry3_vision_e.kVisionLine)
print("Sentry3.VisionBegin(sentry3_vision_e.kVisionLine):0x%x"% err)

def Go_Straight():
  print("Go Straight")
    
def Left():
  print("Left")
  
def Right():
  print("Right")
  
def Hard_Left():
  print("Hard Left")
  
def Hard_Right():
  print("Hard Right")
  
def Stop():
  print("Stop")
        
while True:
    # Sentry3不主动返回检测识别结果，需要主控板发送指令进行读取。读取的流程：首先读取识别结果的数量，接收到指令后，Sentry3会刷新结果数据，如果结果数量不为零，那么主控再发送指令读取结果的相关信息。请务必按此流程构建程序。
    obj_num = Sentry3.GetValue(sentry3_vision_e.kVisionLine, sentry_obj_info_e.kStatus)
    if obj_num:
        # 按照默认设置只巡一条线，所以只需要读取结果1的信息并进行处理即可
        angle = (Sentry3.GetValue(sentry3_vision_e.kVisionLine,sentry_obj_info_e.kLabel,1))
        print("angle= %d"%(angle))
        if (45 > angle):
          Hard_Right()
        if ((80 > angle) and (angle > 45)):
          Right()
        if ((100 > angle) and (angle > 80)):
          Go_Straight()
        if ((135 > angle) and (angle > 100)):
          Left()
        if (angle > 135):
          Hard_Left()
    else:
        Stop()
        
    time.sleep(0.2)    
    print("\n")
